/**
 * Snowflake MEGA sumo robot test nr.0 - simple straight drive test
 * Copyright 2013 Maris BECS <maris.becs@gmail.com>
 */

#include <asf.h>
#include "test0.h"
#include "keyboard.h"
#include "sensors.h"
#include "motors.h"


void test0_app(void)
{
	int8_t speed = 0;
	int8_t frc = -10;	// forward speed correction for right motor
	struct keyboard_event input;

	gfx_mono_draw_string("ENTER - 50%, UP - 30%", 0, 14, &sysfont);
	gfx_mono_draw_string("      DOWN - 100% PWM", 0, 24, &sysfont);

	// Set the PWM
	while (true) {
		keyboard_get_key_state(&input);
		if (input.type == KEYBOARD_RELEASE) {
			if (input.keycode == KEYBOARD_ENTER) {
				speed = 50;		// ENTER - 50% PWM
				frc = -5;	// forward speed correction for right motor
				break;
			} else if (input.keycode == KEYBOARD_UP) {
				speed = 30;		// UP - 30% PWM
				frc = 0;	// forward speed correction for right motor
				break;
			} else if (input.keycode == KEYBOARD_DOWN) {
				speed = 100;	// ENTER - 100% PWM
				break;
			} else if (input.keycode == KEYBOARD_BACK) {
				// set IR module back to OFF state
				ioport_set_pin_high(START_SW);
				// go back to FIGHT menu
				return;
			}				
		}
	}

	// clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
	// show some info
	gfx_mono_draw_string("Test 0      ACTIVATED", 0, 0, &sysfont);
	gfx_mono_draw_string("       3 sec to start", 0, 24, &sysfont);
	gpio_set_pin_low(NHD_C12832A1Z_BACKLIGHT);
	// 3 sec delay before the start
	delay_s(1);
	gfx_mono_draw_string("       2 sec         ", 0, 24, &sysfont);
	gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);
	delay_s(1);
	gfx_mono_draw_string("       1 sec         ", 0, 24, &sysfont);
	gpio_set_pin_low(NHD_C12832A1Z_BACKLIGHT);
	delay_s(1);
	gfx_mono_draw_string("                     ", 0, 12, &sysfont);
	gfx_mono_draw_string("      RUNNING =)     ", 0, 24, &sysfont);
	gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);

	// drive straight corrected !!!
	drive( speed, speed+frc );

	while (true)
	{
		// check kill switch state
		if ( LOW == ioport_get_pin_level(KILL_SW) ) {
			// Kill switch activated
			drive(0,0); // break motors
			// set IR module back to OFF state
			ioport_set_pin_high(START_SW);
			// Clear screen
			gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
			stop_motors();  // free motors
			return;
		}

		delay_ms(5);	// stupid while loop delay
		//drive(speed,speed);

		// check front line sensors
		Line_sens = line_read();
		if( (Line_sens&0b00000010) || (Line_sens&0b00000100) ){		// Line_FL or Line_FR
			delay_us(500);	// after 500 usec read line sensors again
			Line_sens = line_read();
			if( (Line_sens&0b00000010) || (Line_sens&0b00000100) ){	// Line_FL or Line_FR
				// break motors
				drive(0,0);
			}
		}


	}
}	